﻿using System;
using System.Collections.Generic;

namespace GeoFits
{
    #region 异常
    /// <summary>
    /// 参数错误异常
    /// </summary>
    class ParametersWrongException : ApplicationException
    {
        public ParametersWrongException(string message) :
            base(message)
        { }
    }
    #endregion
    /// <summary>
    /// 拟合结果类
    /// </summary>
    public class FitResult
    {
        /// <summary>
        /// 拟合结果
        /// </summary>
        public Geometry ResultGeometry;
        /// <summary>
        /// 在特殊情况下，可能拟合结果不止一个几何体，此时会用到此变量
        /// </summary>
        public List<Geometry> GeometryList;
        /// <summary>
        /// 每一个未知数的残差中误差
        /// </summary>
        public Matrix Qxx;
        /// <summary>
        /// 每一次测量的残差中误差
        /// </summary>
        public Matrix Qvv;
        /// <summary>
        /// 参差向量
        /// </summary>
        public Matrix V;
        /// <summary>
        /// 单位权中误差
        /// </summary>
        public double sigma0;
        /// <summary>
        /// 有时拟合会返回一些其他参数，放于此处
        /// </summary>
        public List<object> Parameters;
    }
    /// <summary>
    /// 拟合中常用的模式
    /// </summary>
    public class FitPattern
    {        
        /// <summary>
        /// 默认精度，如果精度不设置或者设置为零或负数时使用此精度
        /// </summary>
        public static double DEFAULT_PRECISION = 0.000001;
        
        /// <summary>
        /// 函数模型委托
        /// 此委托用于函数模型填充A矩阵和L矩阵
        /// </summary>
        /// <param name="pts">输入的点串</param>
        /// <param name="A">系数矩阵A</param>
        /// <param name="L">常数矩阵L</param>
        /// <param name="m0">未知数的初始值</param>
        /// <param name="conditions">限制条件</param>
        public delegate void FunctionModuleDelegate(List<Point> pts, out Matrix A, out Matrix L, Matrix m0 = null, params double[] conditions);
        
        /// <summary>
        /// 线性化后的函数终止循环条件的委托
        /// </summary>
        /// <param name="m0">计算前未知向量</param>
        /// <param name="m1">计算后未知向量</param>
        /// <returns>如果达到终止循环条件返回true
        /// 否则返回false</returns>
        public delegate bool LinearizedModuleBreakDelegate(Matrix m0, Matrix m1);
        /// <summary>
        /// 线性化后的函数终止循环的默认条件
        /// </summary>
        /// <param name="m0">计算前未知向量</param>
        /// <param name="m1">计算后未知向量</param>
        /// <returns>如果达到终止循环条件返回true
        /// 否则返回false</returns>
        public static bool defaultBreak(Matrix m0, Matrix m1)
        {
            int i = m0.NoRows;
            bool flag = true;
            for (int m = 0; m < i; m++)
            {
                if (Math.Abs(m0[m, 0] - m1[m, 0]) > DEFAULT_PRECISION)
                {
                    flag = false;
                    break;
                }
            }
            return flag;
        }
        
        /// <summary>
        /// 线性模型计算
        /// </summary>
        /// <param name="pts">输入点串</param>
        /// <param name="thisModule">相应的模型委托</param>
        /// <param name="P">权阵</param>
        /// <param name="config">抗差最小二乘法配置
        /// 默认值为null，表示不使用，此参数的具体使用方法可以参照RLSConfig类</param>
        /// <param name="conditions">限制条件</param>
        /// <returns>待求向量</returns>
        public static Matrix LinearModule(List<Point> pts, FunctionModuleDelegate thisModule, Matrix P = null, RLSConfig config = null, params double[] conditions)
        {
            Matrix A, L;
            thisModule(pts, out A, out L, null, conditions);
            if (ReferenceEquals(config, null))
                return Adjustment.SolveIO(A, L, P);
            else
            {
                //除上述的设置外，其他的设置相同，因为对于线性模型只能用抗差最小二乘的线性运算模式
                Matrix PA = null;
                return Adjustment.SolveRLS(A, L, config.getWeight, ref PA, P, null, config.WeightConditions);
            }
                
        }
        /// <summary>
        /// 线性并带精度评定模型计算
        /// </summary>
        /// <param name="pts">输入点串</param>
        /// <param name="thisModule">相应的模型委托</param>
        /// <param name="sigma0">单位权中误差</param>
        /// <param name="Qxx">未知数X的协因数矩阵</param>
        /// <param name="Qvv">每个方程改正数V的协因数矩阵</param>
        /// <param name="V">方程参差向量</param>
        /// <param name="P">权阵</param>
        /// <param name="config">抗差最小二乘法配置
        /// 默认值为null，表示不使用，此参数的具体使用方法可以参照RLSConfig类</param>
        /// <param name="conditions">限制条件</param>
        /// <returns>待求向量</returns>
        public static Matrix LinearModule(List<Point> pts, FunctionModuleDelegate thisModule, out double sigma0, out Matrix Qxx, out Matrix Qvv, out Matrix V, Matrix P = null, RLSConfig config = null, params double[] conditions)
        {
            Matrix A, L;
            thisModule(pts, out A, out L, null, conditions);
            //return Adjustment.SolveIO(A, L, out sigma0, out Qxx, out Qvv, P);
            if (ReferenceEquals(config, null))
                return Adjustment.SolveIO(A, L, out sigma0, out Qxx, out Qvv, out V, P);
            else
            {
                //除上述的设置外，其他的设置相同，因为对于线性模型只能用抗差最小二乘的线性运算模式
                Matrix PA = null;
                return Adjustment.SolveRLS(A, L, config.getWeight, out sigma0, out Qxx, out Qvv, out V, ref PA, P, null, config.WeightConditions);
            }
        }
        
        /// <summary>
        /// 非线性模型线性化后计算
        /// </summary>
        /// <param name="pts">输入点串</param>
        /// <param name="m0">初始值</param>
        /// <param name="thisModule">相应的模型委托</param>
        /// <param name="thisBreak">循环中断条件委托</param>
        /// <param name="P">权阵</param>
        /// <param name="config"></param>
        /// <param name="conditions">限制条件</param>
        /// <returns>待求向量</returns>
        public static Matrix LinearizedModule(List<Point> pts, Matrix m0, FunctionModuleDelegate thisModule, LinearizedModuleBreakDelegate thisBreak = null, Matrix P = null, RLSConfig config = null, params double[] conditions)
        {
            Matrix m1;
            if (ReferenceEquals(thisBreak,null))
            {
                thisBreak = defaultBreak;
            }
            Matrix A, L;

            if (ReferenceEquals(config, null))
            {
                while (true)
                {
                    thisModule(pts, out A, out L, m0, conditions);
                    m1 = Adjustment.SolveIO(A, L, P) + m0;
                    if (thisBreak(m0, m1))
                    {
                        break;
                    }
                    m0 = m1;
                }
                return m1;
            }                
            else
            {
                //进行抗差最小二乘运算
                //Matrix A, L;
                //用抗差估计获得初始值
                thisModule(pts, out A, out L, m0, conditions);
                Matrix PA = null;
                m0 = Adjustment.SolveRLS(A, L, config.getP0, ref PA, P, null, config.P0Conditions) + m0;
                int count = 0;
                while (true)
                {
                    thisModule(pts, out A, out L, m0, conditions);
                    //这里为第二次使用，当然应该用后验权
                    m1 = Adjustment.SolveRLS(A, L, config.getWeight, ref PA, P, null, config.WeightConditions) + m0;
                    count++;
                    if (thisBreak(m0, m1) || count == config.Loops)
                    {
                        break;
                    }
                    m0 = m1;
                }
                return m1;
            }
        }
        /// <summary>
        /// 非线性模型线性化后计算并带精度评定
        /// </summary>
        /// <param name="pts">输入点串</param>
        /// <param name="m0">初始值</param>
        /// <param name="thisModule">相应的模型委托</param>
        /// <param name="thisBreak">循环中断条件委托</param>
        /// <param name="sigma0">单位权中误差</param>
        /// <param name="Qxx">未知数X的协因数矩阵</param>
        /// <param name="Qvv">每个方程改正数V的协因数矩阵</param>
        /// <param name="V">改正数V矩阵</param>
        /// <param name="P">权阵</param>
        /// <param name="config"></param>
        /// <param name="conditions">限制条件</param>
        /// <returns>待求向量</returns>
        public static Matrix LinearizedModule(List<Point> pts, Matrix m0, FunctionModuleDelegate thisModule, out double sigma0, out Matrix Qxx, out Matrix Qvv, out Matrix V, LinearizedModuleBreakDelegate thisBreak = null, Matrix P = null, RLSConfig config = null, params double[] conditions)
        {
            Matrix m1;
            if (ReferenceEquals(thisBreak, null))
            {
                thisBreak = defaultBreak;
            }

            Matrix A, L;
            if (ReferenceEquals(config, null))
            {
                //不使用抗差最小二乘估计
                while (true)
                {                    
                    thisModule(pts, out A, out L, m0, conditions);
                    m1 = Adjustment.SolveIO(A, L, P) + m0;
                    if (thisBreak(m0, m1))
                    {
                        //再计算一次，进行精度评定
                        Adjustment.SolveIO(A, L, out sigma0, out Qxx, out Qvv, out V, P);                    
                        break;
                    }
                    m0 = m1;
                }
                return m1;
            }
            else
            {
                //进行抗差最小二乘运算
                //Matrix A, L;
                //用抗差估计获得初始值
                thisModule(pts, out A, out L, m0, conditions);
                Matrix PA = null;
                m0 = Adjustment.SolveRLS(A, L, config.getP0, ref PA, P, null, config.P0Conditions) + m0;
                int count = 0;
                while (true)
                {
                    thisModule(pts, out A, out L, m0, conditions);
                    m1 = Adjustment.SolveRLS(A, L, config.getWeight, ref PA, P, null, config.WeightConditions) + m0;
                    count++;
                    if (thisBreak(m0, m1) || count == config.Loops)
                    {
                        //在结束循环之前进行一次精度评定
                        Adjustment.SolveRLS(A, L, config.getWeight, out sigma0, out Qxx, out Qvv, out V, ref PA, P, null, config.WeightConditions);
                        break;
                    }
                    m0 = m1;
                }
                return m1;
            }            
        }

        /// <summary>
        /// 分组计算模型
        /// </summary>
        /// <param name="ptgps">输入点串组</param>
        /// <param name="m0">初始值</param>
        /// <param name="moduleList">与点串组相应的模型委托组</param>
        /// <param name="thisBreak">循环中断条件委托</param>
        /// <param name="P">权阵</param>
        /// <param name="config"></param>
        /// <param name="conditions">限制条件</param>
        /// <returns>待求向量</returns>
        public static Matrix GroupedModule(List<List<Point>> ptgps, Matrix m0, List<FunctionModuleDelegate> moduleList, LinearizedModuleBreakDelegate thisBreak = null, Matrix P = null, RLSConfig config = null, params double[] conditions)
        {
            Matrix m1;
            if (ReferenceEquals(thisBreak, null))
            {
                thisBreak = defaultBreak;
            }
            if (moduleList.Count != ptgps.Count)
            {
                throw new ParametersWrongException("点组数与委托数必须相等！");
            }

            Matrix A, L;
            if (ReferenceEquals(config, null))
            {
                while (true)
                {                    
                    getGroupModuleAL(ptgps, moduleList, m0, out A, out L, conditions);
                    m1 = Adjustment.SolveIO(A, L, P) + m0;
                    if (thisBreak(m0, m1))
                    {
                        break;
                    }
                    m0 = m1;
                }
                return m1;
            }                
            else
            {
                //进行抗差最小二乘运算
                //Matrix A, L;
                //用抗差估计获得初始值
                getGroupModuleAL(ptgps, moduleList, m0, out A, out L, conditions);
                Matrix PA = null;
                m0 = Adjustment.SolveRLS(A, L, config.getP0, ref PA, P, null, config.P0Conditions) + m0;
                int count = 0;
                while (true)
                {
                    getGroupModuleAL(ptgps, moduleList, m0, out A, out L, conditions);
                    //thisModule(pts, out A, out L, m0, conditions);
                    m1 = Adjustment.SolveRLS(A, L, config.getWeight, ref PA, P, null, config.WeightConditions) + m0;
                    count++;
                    if (thisBreak(m0, m1) || count == config.Loops)
                    {
                        break;
                    }
                    m0 = m1;
                }
                return m1;
            }
        }

        /// <summary>
        /// 非线性模型线性化后计算
        /// </summary>
        /// <param name="ptgps">输入点串组</param>
        /// <param name="m0">初始值</param>
        /// <param name="moduleList">与点串组相应的模型委托组</param>
        /// <param name="thisBreak">循环中断条件委托</param>
        /// <param name="sigma0">单位权中误差</param>
        /// <param name="Qxx">未知数X的协因数矩阵</param>
        /// <param name="Qvv">每个方程改正数V的协因数矩阵</param>
        /// <param name="V">方程参差向量</param>
        /// <param name="P">权阵</param>
        /// <param name="config">抗差最小二乘法配置
        /// 默认值为null，表示不使用，此参数的具体使用方法可以参照RLSConfig类</param>
        /// <param name="conditions">限制条件</param>
        /// <returns>待求向量</returns>
        public static Matrix GroupedModule(List<List<Point>> ptgps, Matrix m0, List<FunctionModuleDelegate> moduleList, out double sigma0, out Matrix Qxx, out Matrix Qvv, out Matrix V, LinearizedModuleBreakDelegate thisBreak = null, Matrix P = null, RLSConfig config = null, params double[] conditions)
        {
            Matrix m1;
            if (ReferenceEquals(thisBreak, null))
            {
                thisBreak = defaultBreak;
            }
            if (moduleList.Count != ptgps.Count)
            {
                throw new ParametersWrongException("点组数与委托数必须相等！");
            }
            Matrix A, L;
            if (ReferenceEquals(config, null))
            {
                //不使用抗差最小二乘估计时，再计算一次，进行精度评定
                while (true)
                {                    
                    getGroupModuleAL(ptgps, moduleList, m0, out A, out L, conditions);
                    m1 = Adjustment.SolveIO(A, L, P) + m0;
                    if (thisBreak(m0, m1))
                    {
                        //再计算一次，进行精度评定
                        Adjustment.SolveIO(A, L, out sigma0, out Qxx, out Qvv, out V, P);
                        break;
                    }
                    m0 = m1;
                }
                return m1;
            }
            else
            {
                //进行抗差最小二乘运算
                //Matrix A, L;
                //用抗差估计获得初始值
                getGroupModuleAL(ptgps, moduleList, m0, out A, out L, conditions);
                Matrix PA = null;
                m0 = Adjustment.SolveRLS(A, L, config.getP0, ref PA, P, null) + m0;
                int count = 0;
                while (true)
                {
                    getGroupModuleAL(ptgps, moduleList, m0, out A, out L, conditions);
                    m1 = Adjustment.SolveRLS(A, L, config.getWeight, ref PA, P, null) + m0;
                    count++;
                    if (thisBreak(m0, m1) || count == config.Loops)
                    {
                        //在结束循环之前进行一次精度评定
                        Adjustment.SolveRLS(A, L, config.getWeight, out sigma0, out Qxx, out Qvv, out V, ref PA, P, null);
                        break;
                    }
                    m0 = m1;
                }
                return m1;
            }
        }

        private static void getGroupModuleAL(List<List<Point>> ptgps, List<FunctionModuleDelegate> moduleList, Matrix m0, out Matrix A, out Matrix L, double[] conditions)
        {
            List<Matrix> aList = new List<Matrix>();
            List<Matrix> lList = new List<Matrix>();
            int count = 0;
            int n = ptgps.Count;
            for (int i = 0; i < n; i++)
            {
                var pts = ptgps[i];
                var thisModule = moduleList[i];
                Matrix AA, LL;
                thisModule(pts, out AA, out LL, m0, conditions);
                aList.Add(AA);
                lList.Add(LL);
                count += AA.NoRows;
            }
            A = new Matrix(count, aList[0].NoCols);
            L = new Matrix(count, 1);
            int j = 0;
            for (int i = 0; i < n; i++)
            {
                for (int m = 0; m < aList[i].NoRows; m++)
                {
                    for (int k = 0; k < aList[i].NoCols; k++)
                    {
                        A[j, k] = aList[i][m, k];
                    }
                    L[j, 0] = lList[i][m, 0];
                    j++;
                }
            }
        }
    }
}
